/*
 * icm_42605.hpp
 *
 *  Created on: Dec 2, 2021
 *      Author: Administrator
 */

#ifndef INC_ICM42605_HPP_
#define INC_ICM42605_HPP_


// Bank 0
#define ICM42605_DEVICE_CONFIG             0x11
#define ICM42605_DRIVE_CONFIG              0x13
#define ICM42605_INT_CONFIG                0x14
#define ICM42605_FIFO_CONFIG               0x16
#define ICM42605_TEMP_DATA1                0x1D
#define ICM42605_TEMP_DATA0                0x1E
#define ICM42605_ACCEL_DATA_X1             0x1F
#define ICM42605_ACCEL_DATA_X0             0x20
#define ICM42605_ACCEL_DATA_Y1             0x21
#define ICM42605_ACCEL_DATA_Y0             0x22
#define ICM42605_ACCEL_DATA_Z1             0x23
#define ICM42605_ACCEL_DATA_Z0             0x24
#define ICM42605_GYRO_DATA_X1              0x25
#define ICM42605_GYRO_DATA_X0              0x26
#define ICM42605_GYRO_DATA_Y1              0x27
#define ICM42605_GYRO_DATA_Y0              0x28
#define ICM42605_GYRO_DATA_Z1              0x29
#define ICM42605_GYRO_DATA_Z0              0x2A
#define ICM42605_TMST_FSYNCH               0x2B
#define ICM42605_TMST_FSYNCL               0x2C
#define ICM42605_INT_STATUS                0x2D
#define ICM42605_FIFO_COUNTH               0x2E
#define ICM42605_FIFO_COUNTL               0x2F
#define ICM42605_FIFO_DATA                 0x30
#define ICM42605_APEX_DATA0                0x31
#define ICM42605_APEX_DATA1                0x32
#define ICM42605_APEX_DATA2                0x33
#define ICM42605_APEX_DATA3                0x34
#define ICM42605_APEX_DATA4                0x35
#define ICM42605_APEX_DATA5                0x36
#define ICM42605_INT_STATUS2               0x37
#define ICM42605_INT_STATUS3               0x38
#define ICM42605_SIGNAL_PATH_RESET         0x4B
#define ICM42605_INTF_CONFIG0              0x4C
#define ICM42605_INTF_CONFIG1              0x4D
#define ICM42605_PWR_MGMT0                 0x4E
#define ICM42605_GYRO_CONFIG0              0x4F
#define ICM42605_ACCEL_CONFIG0             0x50
#define ICM42605_GYRO_CONFIG1              0x51
#define ICM42605_GYRO_ACCEL_CONFIG0        0x52
#define ICM42605_ACCEL_CONFIG1             0x53
#define ICM42605_TMST_CONFIG               0x54
#define ICM42605_APEX_CONFIG0              0x56
#define ICM42605_SMD_CONFIG                0x57
#define ICM42605_FIFO_CONFIG1              0x5F
#define ICM42605_FIFO_CONFIG2              0x60
#define ICM42605_FIFO_CONFIG3              0x61
#define ICM42605_FSYNC_CONFIG              0x62
#define ICM42605_INT_CONFIG0               0x63
#define ICM42605_INT_CONFIG1               0x64
#define ICM42605_INT_SOURCE0               0x65
#define ICM42605_INT_SOURCE1               0x66
#define ICM42605_INT_SOURCE3               0x68
#define ICM42605_INT_SOURCE4               0x69
#define ICM42605_FIFO_LOST_PKT0            0x6C
#define ICM42605_FIFO_LOST_PKT1            0x6D
#define ICM42605_SELF_TEST_CONFIG          0x70
#define ICM42605_WHO_AM_I                  0x75
#define ICM42605_REG_BANK_SEL              0x76

// Bank 1
#define ICM42605_SENSOR_CONFIG0            0x03
#define ICM42605_GYRO_CONFIG_STATIC2       0x0B
#define ICM42605_GYRO_CONFIG_STATIC3       0x0C
#define ICM42605_GYRO_CONFIG_STATIC4       0x0D
#define ICM42605_GYRO_CONFIG_STATIC5       0x0E
#define ICM42605_GYRO_CONFIG_STATIC6       0x0F
#define ICM42605_GYRO_CONFIG_STATIC7       0x10
#define ICM42605_GYRO_CONFIG_STATIC8       0x11
#define ICM42605_GYRO_CONFIG_STATIC9       0x12
#define ICM42605_GYRO_CONFIG_STATIC10      0x13
#define ICM42605_XG_ST_DATA                0x5F
#define ICM42605_YG_ST_DATA                0x60
#define ICM42605_ZG_ST_DATA                0x61
#define ICM42605_TMSTVAL0                  0x62
#define ICM42605_TMSTVAL1                  0x63
#define ICM42605_TMSTVAL2                  0x64
#define ICM42605_INTF_CONFIG4              0x7A
#define ICM42605_INTF_CONFIG5              0x7B
#define ICM42605_INTF_CONFIG6              0x7C

// Bank 2
#define ICM42605_ACCEL_CONFIG_STATIC2      0x03
#define ICM42605_ACCEL_CONFIG_STATIC3      0x04
#define ICM42605_ACCEL_CONFIG_STATIC4      0x05
#define ICM42605_XA_ST_DATA                0x3B
#define ICM42605_YA_ST_DATA                0x3C
#define ICM42605_ZA_ST_DATA                0x3D

// Bank 4
#define ICM42605_GYRO_ON_OFF_CONFIG        0x0E
#define ICM42605_APEX_CONFIG1              0x40
#define ICM42605_APEX_CONFIG2              0x41
#define ICM42605_APEX_CONFIG3              0x42
#define ICM42605_APEX_CONFIG4              0x43
#define ICM42605_APEX_CONFIG5              0x44
#define ICM42605_APEX_CONFIG6              0x45
#define ICM42605_APEX_CONFIG7              0x46
#define ICM42605_APEX_CONFIG8              0x47
#define ICM42605_APEX_CONFIG9              0x48
#define ICM42605_ACCEL_WOM_X_THR           0x4A
#define ICM42605_ACCEL_WOM_Y_THR           0x4B
#define ICM42605_ACCEL_WOM_Z_THR           0x4C
#define ICM42605_INT_SOURCE6               0x4D
#define ICM42605_INT_SOURCE7               0x4E
#define ICM42605_INT_SOURCE8               0x4F
#define ICM42605_INT_SOURCE9               0x50
#define ICM42605_INT_SOURCE10              0x51
#define ICM42605_OFFSET_USER0              0x77
#define ICM42605_OFFSET_USER1              0x78
#define ICM42605_OFFSET_USER2              0x79
#define ICM42605_OFFSET_USER3              0x7A
#define ICM42605_OFFSET_USER4              0x7B
#define ICM42605_OFFSET_USER5              0x7C
#define ICM42605_OFFSET_USER6              0x7D
#define ICM42605_OFFSET_USER7              0x7E
#define ICM42605_OFFSET_USER8              0x7F

#define ICM42605_ADDRESS                   0x69   // Address of ICM42605 accel/gyro when ADO = HIGH


#define AFS_2G  0x03
#define AFS_4G  0x02
#define AFS_8G  0x01
#define AFS_16G 0x00  // default

#define GFS_2000DPS   0x00 // default
#define GFS_1000DPS   0x01
#define GFS_500DPS    0x02
#define GFS_250DPS    0x03
#define GFS_125DPS    0x04
#define GFS_62_5DPS   0x05
#define GFS_31_25DPS  0x06
#define GFS_15_125DPS 0x07

#define AODR_8000Hz   0x03
#define AODR_4000Hz   0x04
#define AODR_2000Hz   0x05
#define AODR_1000Hz   0x06 // default
#define AODR_200Hz    0x07
#define AODR_100Hz    0x08
#define AODR_50Hz     0x09
#define AODR_25Hz     0x0A
#define AODR_12_5Hz   0x0B
#define AODR_6_25Hz   0x0C
#define AODR_3_125Hz  0x0D
#define AODR_1_5625Hz 0x0E
#define AODR_500Hz    0x0F

#define GODR_8000Hz  0x03
#define GODR_4000Hz  0x04
#define GODR_2000Hz  0x05
#define GODR_1000Hz  0x06 // default
#define GODR_200Hz   0x07
#define GODR_100Hz   0x08
#define GODR_50Hz    0x09
#define GODR_25Hz    0x0A
#define GODR_12_5Hz  0x0B
#define GODR_500Hz   0x0F

#include "main.h"

class icm42605 {

public:
    struct IMUdata {
        float Acce[3];
        float Gyro[3];
    } *data = new IMUdata;

    void setI2C(I2C_HandleTypeDef *hi2c,uint8_t addr)
    {
        this->i2cHandle = hi2c;
        this->i2cAddr = addr << 1;
    }

    void reset(void)
    {
        WriteByte(ICM42605_DEVICE_CONFIG, 0x01);
        HAL_Delay(50);
    }

    bool init(uint8_t GYRO_ODR, uint8_t ACCE_ODR, uint8_t GYRO_RANGE, uint8_t ACCE_RANGE)
    {
        uint8_t temp;

        if (getChipID() != 0x42)
        {
            return false;
        }

        WriteByte(ICM42605_PWR_MGMT0, 0x0F);
        delay_ms(50);

        temp = 0x06;
        setGyroRange(GYRO_RANGE);
        WriteByte(ICM42605_GYRO_CONFIG0, temp | GYRO_ODR | (GYRO_RANGE << 5));
        setAcceRange(ACCE_RANGE);
        WriteByte(ICM42605_ACCEL_CONFIG0, temp | ACCE_ODR | (ACCE_RANGE << 5));

        return true;
    }

    bool init(uint8_t GYRO_RANGE, uint8_t ACCE_RANGE)
    {
        uint8_t temp;

        uint8_t GYRO_ODR, ACCE_ODR;

        GYRO_ODR = GODR_1000Hz;
        ACCE_ODR = AODR_1000Hz;

        if (getChipID() != 0x42) {
            return false;
        }

        WriteByte(ICM42605_PWR_MGMT0, 0x0F);
        delay_ms(50);

        temp = 0x06;
        setGyroRange(GYRO_RANGE);
        WriteByte(ICM42605_GYRO_CONFIG0, temp | GYRO_ODR | (GYRO_RANGE << 5));
        setAcceRange(ACCE_RANGE);
        WriteByte(ICM42605_ACCEL_CONFIG0, temp | ACCE_ODR | (ACCE_RANGE << 5));

        return true;
    }

    bool init()
    {
        uint8_t temp;

        uint8_t GYRO_ODR, ACCE_ODR;
        uint8_t GYRO_RANGE, ACCE_RANGE;

        GYRO_RANGE = GFS_2000DPS;
        ACCE_RANGE = AFS_16G;

        GYRO_ODR = GODR_1000Hz;
        ACCE_ODR = AODR_1000Hz;

        this->reset();
        this->delay_ms(200);

        if (getChipID() != 0x42)
        {
            return false;
        }

        WriteByte(ICM42605_PWR_MGMT0, 0x0F);
        delay_ms(50);

        temp = 0x06;
        setGyroRange(GYRO_RANGE);
        WriteByte(ICM42605_GYRO_CONFIG0, temp | GYRO_ODR | (GYRO_RANGE << 5));
        setAcceRange(ACCE_RANGE);
        WriteByte(ICM42605_ACCEL_CONFIG0, temp | ACCE_ODR | (ACCE_RANGE << 5));

        return true;
    }


    void Updata()
    {
        uint8_t *data = (uint8_t*)this->RawData;
        uint8_t temp;
        ReadBytes(ICM42605_ACCEL_DATA_X1, data, 12);

        for (int i = 0; i < 3; ++i)
        {
            temp = data[i*2];
            data[i*2] = data[i*2+1];
            data[i*2+1] = temp;

            temp = data[i*2+6];
            data[i*2+6] = data[i*2+7];
            data[i*2+7] = temp;


            this->data->Acce[i] = (float) this->RawData[i] * AcceLSB;
            this->data->Gyro[i] = (float) this->RawData[3 + i] * GyroLSB;
        }
    };

    uint8_t getChipID(void)
    {
        // Chip id 为0x42
        uint8_t temp = ReadByte(ICM42605_WHO_AM_I);
        return temp;
    }


protected:

    I2C_HandleTypeDef *i2cHandle;
    uint8_t i2cAddr;

    float GyroLSB;
    float AcceLSB;
    // RawData * LSB = Data

    int16_t RawData[6];


    void delay_ms(uint32_t delayTime)
    {
        HAL_Delay(delayTime);
    }

    uint8_t ReadByte(uint8_t reg)
    {
        uint8_t *rxdata = new uint8_t;

        HAL_I2C_Mem_Read(this->i2cHandle,this->i2cAddr,(uint16_t)reg,1,rxdata,1,5);

        return rxdata[0];
    }

    void ReadBytes(uint8_t start_reg, uint8_t *array, uint8_t size)
    {
        for (uint8_t i = 0; i < size; ++i)
        {
            array[i] = ReadByte(start_reg + i);
        }
    }

    void WriteByte(uint8_t reg, uint8_t data)
    {
        HAL_I2C_Mem_Write(this->i2cHandle,this->i2cAddr,(uint16_t)reg,1,&data,1,5);
    }


    float setAcceRange(uint8_t AcceRange)
    {
        switch (AcceRange)
        {
            case AFS_2G:
                AcceLSB = 2.0f / 32768.0f;
                return AcceLSB;
                break;
            case AFS_4G:
                AcceLSB = 4.0f / 32768.0f;
                return AcceLSB;
                break;
            case AFS_8G:
                AcceLSB = 8.0f / 32768.0f;
                return AcceLSB;
                break;
            case AFS_16G:
                AcceLSB = 16.0f / 32768.0f;
                return AcceLSB;
                break;
        }
    }

    float setGyroRange(uint8_t GyroRange)
    {
        switch (GyroRange)
        {
            case GFS_15_125DPS:
                GyroLSB = 15.125f / 32768.0f;
                return GyroLSB;
                break;
            case GFS_31_25DPS:
                GyroLSB = 31.25f / 32768.0f;
                return GyroLSB;
                break;
            case GFS_62_5DPS:
                GyroLSB = 62.5f / 32768.0f;
                return GyroLSB;
                break;
            case GFS_125DPS:
                GyroLSB = 125.0f / 32768.0f;
                return GyroLSB;
                break;
            case GFS_250DPS:
                GyroLSB = 250.0f / 32768.0f;
                return GyroLSB;
                break;
            case GFS_500DPS:
                GyroLSB = 500.0f / 32768.0f;
                return GyroLSB;
                break;
            case GFS_1000DPS:
                GyroLSB = 1000.0f / 32768.0f;
                return GyroLSB;
                break;
            case GFS_2000DPS:
                GyroLSB = 2000.0f / 32768.0f;
                return GyroLSB;
                break;
        }
    }
};

#endif /* INC_ICM42605_HPP_ */
